#include <opencv2/opencv.hpp>
#include <iostream>
#include <cmath>

using namespace std;
using namespace cv;

vector<int> HSV{0, 137, 100, 255, 154, 255}; //符文hsv
double RateWidthDistance = 3500;
double dRateWidthDistance = 2500;
double maxNoise = 400;
double camera_matrix[3][3] = {{1.6041191539594568e+03, 0., 6.3983687194220943e+02}, {0., 1.6047833493341714e+03, 5.1222951297937527e+02}, {0., 0., 1.}};
double dist[5] = {-6.4910709385278609e-01, 8.6914328787426987e-01,
				  5.1733428362687644e-03, -4.1111054148847371e-03, 0.};
Mat cameraMatrix(3, 3, cv::DataType<double>::type, camera_matrix);
Mat distCoeffs(5, 1, cv::DataType<double>::type, dist);
vector<Point3f> obj{{0, 0, 0}, {0, 230, 0}, {230, 127, 0}, {127, 0, 0}}; //从左上开始，顺时针

class lightBar
{
public:
	Point2d center;
	RotatedRect rect;
};
class armor
{
public:
	Point2d center;
	lightBar barLeft;
	lightBar barRight;
};

double pointDistance(Point2d a, Point2d b)
{
	return (sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y)));
}

void mySortRotatedRectPoints(Point2f vetPoints[], RotatedRect rect)
{
	rect.points(vetPoints);
	if (rect.size.width > rect.size.height)
	{
		Point2f temp;
		temp = vetPoints[0];
		vetPoints[0] = vetPoints[2];
		vetPoints[2] = temp;
		temp = vetPoints[1];
		vetPoints[1] = vetPoints[3];
		vetPoints[3] = temp;
	}
	else if (rect.size.width < rect.size.height)
	{
		Point2f temp0, temp1, temp2;
		temp0 = vetPoints[0];
		temp1 = vetPoints[1];
		temp2 = vetPoints[2];
		vetPoints[0] = temp1;
		vetPoints[1] = temp2;
		vetPoints[2] = vetPoints[3];
		vetPoints[3] = temp0;
	}
}
//默认整理顺序为左上 右上 右下 左下
void SortRotatedRectPoints(Point2f vetPoints[], RotatedRect rect, int flag = 0)
{
	rect.points(vetPoints);

	std::cout << vetPoints[0] << vetPoints[1] << vetPoints[2] << vetPoints[3] << endl;
	std::cout << rect.angle << endl;

	Point2f curpoint;
	if (flag == 0)
	{
		//按X轴排序
		for (int i = 0; i < 4; ++i)
		{
			for (int k = i + 1; k < 4; ++k)
			{
				if (vetPoints[i].x > vetPoints[k].x)
				{
					curpoint = vetPoints[i];
					vetPoints[i] = vetPoints[k];
					vetPoints[k] = curpoint;
				}
			}
		}

		//判断X坐标前两个定义左上左下角
		if (vetPoints[0].y > vetPoints[1].y)
		{
			curpoint = vetPoints[0];
			vetPoints[0] = vetPoints[1];
			vetPoints[1] = vetPoints[3];
			vetPoints[3] = curpoint;
		}
		else
		{
			curpoint = vetPoints[3];
			vetPoints[3] = vetPoints[1];
			vetPoints[1] = curpoint;
		}

		//判断X坐标后两个定义右上右下角
		if (vetPoints[1].y > vetPoints[2].y)
		{
			curpoint = vetPoints[1];
			vetPoints[1] = vetPoints[2];
			vetPoints[2] = curpoint;
		}
	}
	else
	{
		//根据Rect的坐标点，Y轴最大的为P[0]，p[0]围着center顺时针旋转,
		//旋转角度为负的话即是P[0]在左下角，为正P[0]是右下角
		//重新排序坐标点
		if (rect.angle < 0)
		{
			curpoint = vetPoints[0];
			vetPoints[0] = vetPoints[2];
			vetPoints[2] = curpoint;
			curpoint = vetPoints[1];
			vetPoints[1] = vetPoints[3];
			vetPoints[3] = curpoint;
		}
		else if (rect.angle > 0)
		{
			curpoint = vetPoints[0];
			vetPoints[0] = vetPoints[1];
			vetPoints[1] = vetPoints[2];
			vetPoints[2] = vetPoints[3];
			vetPoints[3] = curpoint;
		}
	}
}

Mat drawPicture(Mat drawpicture, vector<armor> armors)
{
	Mat frame = drawpicture.clone();
	for (int i = 0; i < armors.size(); i++)
	{
		Point2f sortPointLeft[4];
		Point2f sortPointRight[4];
		mySortRotatedRectPoints(sortPointLeft, armors[i].barLeft.rect);
		mySortRotatedRectPoints(sortPointRight, armors[i].barRight.rect);
		circle(frame, armors[i].center, 6, Scalar(0, 240, 0));
		circle(frame, sortPointLeft[0], 6, Scalar(0, 240, 0));
		circle(frame, sortPointLeft[3], 6, Scalar(0, 240, 0));
		circle(frame, sortPointRight[1], 6, Scalar(0, 240, 0));
		circle(frame, sortPointRight[2], 6, Scalar(0, 240, 0));
		line(frame, sortPointLeft[0], sortPointRight[2], Scalar((255, 255, 255)));
		line(frame, sortPointLeft[3], sortPointRight[1], Scalar((255, 255, 255)));
		line(frame, sortPointRight[1], sortPointRight[2], Scalar((255, 255, 255)));

		return frame;
	}
}

std::vector<armor> createArmor(std::vector<lightBar> &bars)
{
	std::vector<armor> Armors;
	vector<int> index; //存放已经被选择出去的甲板
	bool work1;
	cout << "甲板检测" << endl;
	for (int i = 0; i < bars.size(); i++)
	{
		work1 = 1;
		if (!index.empty())
		{
			for (int j = 0; j < index.size(); j++)
			{
				if (i == index[j])
					work1 = 0;
				break;
			}
		}

		if (work1)
		{
			// cout << "1" << endl;
			int work2 = 1;
			for (int k = 0; k < bars.size(); k++)
			{
				if (i == k)
				{
					work2 = 0;
					continue;
				}
				if (!index.empty())
				{
					// cout << "2" << endl;
					for (int b = 0; b < index.size(); b++)
					{
						if (k == index[b])
							work2 = 0;
						break;
					}
				}
				if (work2)
				{
					double bar1Width = min(bars[i].rect.size.width, bars[i].rect.size.height),
						   bar2Width = min(bars[k].rect.size.width, bars[k].rect.size.height),
						   centerDistance = pointDistance(bars[i].rect.center, bars[k].rect.center);
					// cout << "RateWidthDistance: " << (bar1Width + bar2Width) / 2.0*centerDistance << endl;
					if ((bar1Width + bar2Width) / 2.0 * centerDistance > RateWidthDistance - dRateWidthDistance && (bar1Width + bar2Width) / 2.0 * centerDistance < RateWidthDistance + dRateWidthDistance)
					{
						cout << "甲板加一" << endl;
						index.push_back(i);
						index.push_back(k);
						armor armorTemp;
						armorTemp.center = {(bars[i].rect.center.x + bars[k].rect.center.x) / 2.0, (bars[i].rect.center.y + bars[k].rect.center.y) / 2.0};
						if (bars[i].rect.center.x > bars[k].rect.center.x)
						{
							armorTemp.barLeft = bars[k];
							armorTemp.barRight = bars[i];
						}
						else
						{
							armorTemp.barLeft = bars[i];
							armorTemp.barRight = bars[k];
						}
						Armors.push_back(armorTemp);
					}
				}
			}
		}
	}

	return Armors;
}
vector<lightBar> findLightBars(Mat frameColor)
{
	Mat frameClone = frameColor.clone();
	vector<lightBar> lightBars;

	GaussianBlur(frameClone, frameClone, Size(3, 3), 3);
	Canny(frameClone, frameClone, 100, 150);
	vector<vector<Point>> contours;
	findContours(frameClone, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
	int contourSize = contours.size();
	vector<vector<Point>> conPoly(contourSize);
	vector<RotatedRect> boundingbox(contourSize);

	for (int i = 0; i < contourSize; i++)
	{
		double area = contourArea(contours[i]);
		// cout << "area: " << area << endl;

		if (area > maxNoise)
		{
			lightBar lightbar;
			float perimeter = arcLength(contours[i], true);
			approxPolyDP(contours[i], conPoly[i], 0.02 * perimeter, true); //近似多边形轮廓
			drawContours(frameClone, contours, -1, Scalar(230, 0, 0), 2);  //黄色的识别灯带
			boundingbox[i] = minAreaRect(conPoly[i]);
			lightbar.rect = boundingbox[i];
			lightbar.center = lightbar.rect.center;
			lightBars.push_back(lightbar);
		}
	}
	imshow("轮廓", frameClone);
	return lightBars;
}

Mat findColor(Mat frame)
{
	Mat framehsv, frameArmourColor;
	cvtColor(frame, framehsv, COLOR_BGR2HSV);
	Scalar lower(HSV[0], HSV[2], HSV[4]); //标量定义不用写=
	Scalar upper(HSV[1], HSV[3], HSV[5]);
	inRange(framehsv, lower, upper, frameArmourColor);
	imshow("甲板颜色检测", frameArmourColor);
	return frameArmourColor;
}
void envirInfor(armor Armor, double distance, double yaw, double pitch, double angleX, double angleY, double depth)
{
	vector<Point2f> imgPoints;
	Point2f armorLeft[4], armorRight[4];
	mySortRotatedRectPoints(armorLeft, Armor.barLeft.rect);
	mySortRotatedRectPoints(armorLeft, Armor.barLeft.rect);
	imgPoints.push_back(armorLeft[0]);
	imgPoints.push_back(armorRight[1]);
	imgPoints.push_back(armorRight[2]);
	imgPoints.push_back(armorLeft[3]);

	Mat rVec = cv::Mat::zeros(3, 1, CV_64FC1);
	Mat tVec = cv::Mat::zeros(3, 1, CV_64FC1);

	solvePnP(obj, imgPoints, cameraMatrix, distCoeffs, rVec, tVec, false, SOLVEPNP_ITERATIVE);
	Rodrigues(rVec,rVec);
	double changeindex = (pointDistance(imgPoints[0], imgPoints[1]) + pointDistance(imgPoints[2], imgPoints[3]) + pointDistance(imgPoints[0], imgPoints[3]) + pointDistance(imgPoints[2], imgPoints[1])) / 562.0;
	yaw = -asin(rVec.at<double>(2,0)) / CV_PI * 180.0;
	double roll = acos(rVec.at<double>(2,1) / rVec.at<double>(2,2)) / CV_PI * 180.0;
	pitch = acos(rVec.at<double>(1,0) / rVec.at<double>(0,0)) / CV_PI * 180.0;
	distance = sqrt(tVec.at<double>(0, 0) * tVec.at<double>(0, 0) + tVec.at<double>(1, 0) * tVec.at<double>(1, 0) + tVec.at<double>(2, 0) * tVec.at<double>(2, 0));
	angleX = atan((360 - Armor.center.x) / (tVec.at<double>(2, 0) * changeindex)) / CV_PI * 180.0;
	angleY = atan((240 - Armor.center.x) / (tVec.at<double>(2, 0) * changeindex)) / CV_PI * 180.0;

	cout << "_____________________________________" << endl;
	cout << "欧氏距离为:" << distance << endl;
	cout << "angleX为: " << angleX << endl;
	cout << "angleY为: " << angleY << endl;
	cout << "yaw: " << yaw << endl;
	cout << "pitch: " << pitch << endl;
	cout << "roll: " << roll << endl;
	cout << "_____________________________________" << endl;
}
void predictPoints(Mat frame, vector<armor> &armorsPrevious, vector<armor> &armors, Point2d &predict, double rosLength)
{
	for (int i = 0; i < armors.size(); i++)
	{
		int mindistance = 100000000;
		int minJndex = 0;
		for (int j = 0; j < armorsPrevious.size(); j++)
		{
			double pointdistance = pointDistance(armorsPrevious[i].center, armors[j].center);
			if (pointdistance > rosLength)
				continue;
			if (pointdistance < mindistance)
			{
				mindistance = pointdistance;
				minJndex = j;
			}
		}

		predict = {2.0 * armors[i].center.x - armorsPrevious[i].center.x, 2.0 * armors[i].center.y - armorsPrevious[i].center.y};
		circle(frame, predict, 3, Scalar(255, 255, 0), 1);
	}
}
int main()
{
	VideoCapture cap("task.avi");
	Mat frame;
	vector<armor> armorsPrevious;
	while (1)
	{
		vector<armor> armors;
		vector<lightBar> lightBars;
		Point2d predict;
		cap >> frame;

		double distance = 0, yaw = 0, pitch = 0, angleX = 0, angleY = 0, depth = 0;
		Mat frameArmourColor, frameColor = frame.clone(), frameFinal = frame.clone();
		frameColor = findColor(frame);
		lightBars = findLightBars(frameColor);
		if (lightBars.size() != 0)
		{
			armors = createArmor(lightBars);
			frameFinal = drawPicture(frame, armors);
			if (!armors.empty())
			{
				if (!armorsPrevious.empty())
				{
					predictPoints(frameFinal, armorsPrevious, armors, predict, 40);
				}
				for (int j = 0; j < armors.size(); j++)
				{
					envirInfor(armors[j], distance, yaw, pitch, angleX, angleY, depth);
					/*putText(frameFinal, "深度信息：" + to_string(depth), Point(5, 5), FONT_HERSHEY_SIMPLEX, 5, Scalar(0, 0, 222));
					putText(frameFinal, "关于X轴的偏角：" + to_string(angleX), Point(8, 5), FONT_HERSHEY_SIMPLEX, 1.5, Scalar(0, 0, 222));
					putText(frameFinal, "关于Y轴的偏角：" + to_string(angleY), Point(11, 5), FONT_HERSHEY_SIMPLEX, 1.5, Scalar(0, 0, 222));*/
				}

				armorsPrevious = armors;
			}
		}

		imshow("原图像", frame);
		imshow("识别显示", frameFinal);
		waitKey(30);
	}
}